
/*******************************************************************************
**	INCLUDES
*******************************************************************************/
#include <x32.h>
#include <ucos.h>

/*******************************************************************************
**	DEFINITIONS
*******************************************************************************/

//x32 PERIPHERALS
#define X32_PWM_PERIOD 		peripherals[PERIPHERAL_DPC1_PERIOD]
#define X32_PWM_WIDTH 		peripherals[PERIPHERAL_DPC1_WIDTH]
#define X32_SSD 			peripherals[PERIPHERAL_DISPLAY]
#define X32_ENGINE_A 		peripherals[PERIPHERAL_ENGINE_A]
#define X32_ENGINE_B  		peripherals[PERIPHERAL_ENGINE_B]
#define X32_ENGINE_DECODED	peripherals[PERIPHERAL_ENGINE_DECODED]
#define X32_LEDS			peripherals[PERIPHERAL_LEDS]


// uC/OS PRIORITIES
// higher number is higher priority
#define PERIODIC_TASK_PRIORITY	1
#define ERROR_TASK_PRIORITY 	9
#define BUTTON0_TASK_PRIORITY 	5
#define BUTTON1_TASK_PRIORITY 	6
#define BUTTON2_TASK_PRIORITY 	7
#define BUTTON3_TASK_PRIORITY 	10


//x32 ISR PRIORITIES
// lower number is higher priority
#define PRIORITY_ENGINE_ERROR 	2
#define PRIORITY_BUTTONS 		5

// LEDs Definitions
#define LED7 0x80


// BUTTONS Definitions
#define BTN3 0x08
#define BTN2 0x04
#define BTN1 0x02
#define BTN0 0x01

// Error Definition for InteruptEnableRegister
#define ERR 0x10

// Button State Definitions
#define BTN0_PRESSED 		peripherals[PERIPHERAL_BUTTONS] & BTN0
#define BTN1_PRESSED 		peripherals[PERIPHERAL_BUTTONS] & BTN1
#define BTN2_PRESSED 		peripherals[PERIPHERAL_BUTTONS] & BTN2
#define BTN3_PRESSED 		peripherals[PERIPHERAL_BUTTONS] & BTN3


// uC/OS TIME Definitions
#define TIME_SLICE 		1
#define DEBOUNCE_TIME 	2*TIME_SLICE
#define MS100			5*TIME_SLICE

/*******************************************************************************
**	GLOBAL VARIABLES
*******************************************************************************/
int throttle = 0, old_count = 0, speed = 0;
int stack0[1024], stack1[128], stack2[128], stack3[128], stack4[128], stack5[256];
int interuptEnableReg = 0;
int oldState = 0;
OS_EVENT *sem_error, *sem_btn0, *sem_btn1, *sem_btn2, *sem_btn3;
UBYTE err;



/*******************************************************************************
**	INTERRUPT SERVICE ROUTINES
*******************************************************************************/

void isr_buttons() { 										// Button interupt service
	OSIntEnter();											// OSIntEnter stops OS temporarily to stop context switching	
	
	if (BTN3_PRESSED && !(oldState & BTN3))  {		               				// Check if interupt is enabled
		interuptEnableReg |= BTN3;				// Set interupt flag							
		OSSemPost(sem_btn3);							// Post sem_btn3, any task waiting will be resumed		
	}
	else interuptEnableReg &= ~BTN3;

	
	if (BTN0_PRESSED && !(oldState & BTN0)) {					// Increase button, right most on board.
		interuptEnableReg |= BTN0;				// Set interupt flag
		printf("0");
		OSSemPost(sem_btn0);							// Post sem_btn0, any task waiting will be resumed			
	}
	else interuptEnableReg &= ~BTN0;
	
	if (BTN1_PRESSED && !(oldState & BTN1)) {					// Increase button, right most on board.
		interuptEnableReg |= BTN1;				// Set interupt flag
		OSSemPost(sem_btn1);							// Post sem_btn1, any task waiting will be resumed		
	}
	else interuptEnableReg &= ~BTN1;

	if( BTN0_PRESSED ) oldState |= BTN0;
	else oldState &= ~BTN0; 
	if( BTN1_PRESSED ) oldState |= BTN1;
	else oldState &= ~BTN1; 
	if( BTN3_PRESSED ) oldState |= BTN3;
	else oldState &= ~BTN3; 
	OSIntExit(); 											// Restore OS context switching
}

void isr_error() {
	OSIntEnter();											// OSIntEnter stops OS temporarily to stop context switching	
	if (!(interuptEnableReg & ERR)) {						// Check if interupt is enabled	
		OSSemPost(sem_error);								// Post sem_error, any task waiting will be resumed
		interuptEnableReg |= ERR;					// Set interupt flag
	}
	OSIntExit();											// Restore OS context switching
}	


/*******************************************************************************
**	uC/OS TASKS
*******************************************************************************/

void per_task(void *data) {
	while(TRUE) {
		old_count = X32_ENGINE_DECODED; 			// Get the current count for measuring the change of count
		X32_PWM_WIDTH = throttle;					// Assign the new throttle
		OSTimeDly(TIME_SLICE); 						// 20ms; Delay the system for one timeslice
		
		speed   = X32_ENGINE_DECODED-old_count; 	// Speed is the rate of change of count over an interval TIME_SLICE
		X32_SSD = (speed << 8) + (throttle >> 2);	// Speed displayed on the left and throttle displayed on the right of the SSD
	}												// SSD can hold at most 16bit value.
}

void err_task(void *data) {
	while(TRUE) {
		OSSemPend(sem_error, WAIT_FOREVER, &err); 	// Pend on sem_error semaphore (Wait until posted)
		X32_LEDS |= LED7; 							// Turn on LED7
		OSTimeDly(MS100); 							// wait 100ms
		X32_LEDS &= ~LED7; 							// Turn off LED7
		interuptEnableReg &= ~ERR;
		//OSSemAccept(sem_error);						// Restore sem_error to 0
	}
}

void btn0_task(void *data) {
	while(TRUE) {
		OSSemPend(sem_btn0, WAIT_FOREVER, &err); 	// Pend on sem_btn0 semaphore (Wait until posted)
		OSTimeDly(DEBOUNCE_TIME); 					// Let the OS wait AT LEAST 1 timeslice
		printf("1");
		if (BTN0_PRESSED && ((interuptEnableReg & BTN0))) {							// Check the state of BTN0
			if(throttle <= 0xFFFB) throttle += 4;	// Increase throttle by 4
		}
		//else {
		//	if (!BTN0_PRESSED && (interuptEnableReg & BTN0)) interuptEnableReg &= ~BTN0;
		//}
		//OSSemAccept(sem_btn0);						// Restore sem_btn0 to 0
	}
}

void btn1_task(void *data) {
	while(TRUE) {
		OSSemPend(sem_btn1, WAIT_FOREVER, &err); 	// Pend on sem_btn1 semaphore (Wait until posted)
		OSTimeDly(DEBOUNCE_TIME); 					// Let the OS wait AT LEAST 1 timeslice
		
		if (BTN1_PRESSED && ((interuptEnableReg & BTN1))) {							// Check the state of BTN0
			if(throttle >= 0x0004) throttle -= 4;	// Decrease throttle by 4
		}
		//else {
		//	if (!BTN1_PRESSED && (interuptEnableReg & BTN1)) interuptEnableReg &= ~BTN1;
		//}
		//OSSemAccept(sem_btn1);						// Restore sem_btn1 to 0
	}
}

void btn3_task(void *data) {
	while(TRUE) {
		OSSemPend(sem_btn3, WAIT_FOREVER, &err); 	// Pend on sem_btn3 semaphore (Wait until posted)
		OSTimeDly(DEBOUNCE_TIME); 					// Let the OS wait AT LEAST 1 timeslice
		if (BTN3_PRESSED && ((interuptEnableReg & BTN3))) {							// Check the state of BTN0
			X32_SSD 		= 0x00;					// Reset screen to zeros
			X32_PWM_WIDTH	= 0x00;					// Reset PWM to zeros
			exit();									// Exit OS to bootloader
		}
		//else {
		//	if (!BTN3_PRESSED && (interuptEnableReg & BTN3)) interuptEnableReg &= ~BTN3;
		//}
		//OSSemAccept(sem_btn3);						// Restore sem_btn3 to 0
	}
}

/*******************************************************************************
**	MAIN
*******************************************************************************/

void main() {
	// Create Semaphores
	sem_error = OSSemCreate(0);
	sem_btn0  = OSSemCreate(0);
	sem_btn1  = OSSemCreate(0);
	//sem_btn2  = OSSemCreate(0);
	sem_btn3  = OSSemCreate(0);

	// Configure the ENGINE_ERROR ISR 
	SET_INTERRUPT_VECTOR	(INTERRUPT_ENGINE_ERROR, isr_error);
	SET_INTERRUPT_PRIORITY	(INTERRUPT_ENGINE_ERROR, PRIORITY_ENGINE_ERROR);
	ENABLE_INTERRUPT		(INTERRUPT_ENGINE_ERROR);

	// Configure the BUTTONS ISR
	SET_INTERRUPT_VECTOR	(INTERRUPT_BUTTONS, isr_buttons);
	SET_INTERRUPT_PRIORITY	(INTERRUPT_BUTTONS, PRIORITY_BUTTONS);
	ENABLE_INTERRUPT		(INTERRUPT_BUTTONS);

	X32_PWM_PERIOD = 1024;

	OSInit();	// Init the OS
	
	// Lower number -> Higher priority
	OSTaskCreate(per_task, 	(void *)0, (void *)stack0, PERIODIC_TASK_PRIORITY); // Priority=1
	OSTaskCreate(err_task, 	(void *)0, (void *)stack1, ERROR_TASK_PRIORITY); 	// Priority=9
	OSTaskCreate(btn0_task, (void *)0, (void *)stack2, BUTTON0_TASK_PRIORITY); 	// Priority=5
	OSTaskCreate(btn1_task, (void *)0, (void *)stack3, BUTTON1_TASK_PRIORITY);	// Priority=6
	//OSTaskCreate(btn2_task, (void *)0, (void *)stack4, BUTTON2_TASK_PRIORITY);	// Priority=7
	OSTaskCreate(btn3_task, (void *)0, (void *)stack5, BUTTON3_TASK_PRIORITY);	// Priority=10
	OSStart(); // Also enables the global interrupts
}



